﻿#include "ImgProdCons.h"

#include <signal.h>

#include<opencv2/opencv.hpp>
#include<chrono>
#include<iostream>
#include<thread>
#include<memory>

using namespace std;
using namespace cv;

namespace rm
{

FrameBuffer::FrameBuffer(size_t size):
    _frames(size),
    _mutexs(size),
    _tailIdx(0),
    _headIdx(0),
    _lastGetTimeStamp(0.0)
{

}

bool FrameBuffer::push(const Frame& frame)
{
    const size_t newHeadIdx = (_headIdx + 1) % _frames.size();

    //try for 2ms to lock
    unique_lock<timed_mutex> lock(_mutexs[newHeadIdx],chrono::milliseconds(2));
    if(!lock.owns_lock())
    {
        return false;
    }

    _frames[newHeadIdx] = frame;
    if(newHeadIdx == _tailIdx)
    {
        _tailIdx = (_tailIdx + 1) % _frames.size();
    }
    _headIdx = newHeadIdx;
    return true;
}

bool FrameBuffer::getLatest(Frame& frame)
{
    volatile const size_t headIdx = _headIdx;

    //try for 2ms to lock

    unique_lock<timed_mutex> lock(_mutexs[headIdx],chrono::milliseconds(2));
    if(!lock.owns_lock() ||
       _frames[headIdx].img.empty() ||
       _frames[headIdx].timeStamp == _lastGetTimeStamp)
    {
        return false;
    }

    frame = _frames[headIdx];
    _lastGetTimeStamp = _frames[headIdx].timeStamp;

    return true;
}



ImgProdCons::ImgProdCons():
    _videoCapturePtr(make_unique<DHVideoCapture>()),
    _buffer(6),
    _serialPtr(make_unique<Serial>()),
    _solverPtr(make_unique<AngleSolver>()),
    _enemyDetectorPtr(make_unique<EnemyDetector>()),
    //_runeDetectorPtr(make_unique<RuneDetector>()),
    _windmillPtr(make_unique<Windmill>()),
//    _sentryStrategiesPtr(make_unique<Strategies>()),
    _predictorPtr(make_unique<Predictor>()),
    _task(Serial::NO_TASK),
    bullet_speed(15)
{
    _outfile.open("/home/nuc/runlog.txt", ios::app);
    XInitThreads();
}

void ImgProdCons::signal_handler(int)
{
    _quit_flag = true;
}
void ImgProdCons::init_signals(void)
{
    _quit_flag = false;
    struct sigaction sigact;
    sigact.sa_handler = signal_handler;
    sigemptyset(&sigact.sa_mask);
    sigact.sa_flags = 0;
    sigaction(SIGINT,&sigact,(struct sigaction*)NULL);
}

void ImgProdCons::produce()
{
    auto startTime = chrono::high_resolution_clock::now();
    auto t1 = chrono::high_resolution_clock::now();
    int pre_sec = -1;
    FeedBackData feedBackData;
    GyroData poseEuler;
    for(;;)
    {
        ofstream output;
        output.open("/home/nuc/Robomaster2021/status.log", ios::trunc);
        output << "everything_OK";
        output.close();


        /*
         * Can prevent the camera usb port from being blocked as possible.
         */
        if (_quit_flag) return;

        CFrame frame;
        /*
         * Decoding costs lots of time, so let the STM record after grab,
         * and then decode by calling 'retrieve'.
         */

        if(!_videoCapturePtr->grab(frame)) continue;

        /*
         * Every new Mat has a new address.
         * Let the sequence number of image frame and serial frame be the same.
         */
        Mat newImg;
        uint8_t seq;

        time_t nowtime;
        time(&nowtime);
        _nowtime = localtime(&nowtime);
        if(_nowtime->tm_sec != pre_sec)
        {
            _outfile << "produce:  " << _nowtime->tm_mon+1 << "/" << _nowtime->tm_mday << "  " << _nowtime->tm_hour << ":" << _nowtime->tm_min << ":" << _nowtime->tm_sec << endl;
        }
        pre_sec = _nowtime->tm_sec;

        /*
         * Tell the STM to record.
         */
//        auto t3 = chrono::high_resolution_clock::now();
//        int error = _serialPtr->tryFeedBack(feedBackData, chrono::milliseconds(5));
        int k = 0;
//        if(error != Serial::OJBK)
//        {
//            cout<<"try record fail     "<<dec<<error<<endl;
//            _outfile<<"tryRecord fail:    "<<dec<<error<<endl;
//            if(error ==  Serial::READ_WRITE_ERROR || error == Serial::SYSTEM_ERROR || error == Serial::CORRUPTED_FRAME)
//            {
//                while(k < 3)
//                {
//                    cout << "Try Reopen Port" << k << endl;
//                    _serialPtr->closePort();
//                    sleep(0.5);
//                    _serialPtr->openPort();
//                    k++;
//                    if( _serialPtr->tryFeedBack(feedBackData, chrono::milliseconds(5)) == Serial::OJBK)
//                    {
//                        break;
//                    }
//                }
//            }
//            continue;
//        }
//        else
//        {
//            poseEuler.gimbal_pitch = feedBackData.gimbal_pitch;
//            poseEuler.gimbal_yaw = feedBackData.gimbal_yaw;
//            _task = feedBackData.task_mode;
//            bullet_speed=feedBackData.bullet_speed;
//        }

//        cout<<"try record succeed\n";
//        auto t4 = chrono::high_resolution_clock::now();
//        cout << "Capture period: " << (static_cast<chrono::duration<double, std::milli>>(t4 - t3)).count() << " ms" << endl;
        /*
         * Decode image
         */
        if(!_videoCapturePtr->retrieve(newImg, frame))
        {
//            cout<<"retrieve failed"<<endl;
            _outfile<<"retrieve failed"<<endl;
            continue;
        }
//        if(!_videoCapturePtr->retrieve(newImg)) continue;

        double timeStamp = (static_cast<chrono::duration<double,std::milli>>(chrono::high_resolution_clock::now() - startTime)).count();

        /*
         * push the new image into the circular buffer
         */

        _buffer.push(Frame{newImg, poseEuler,seq, timeStamp});
        //cout<<"push succeed"<<endl;
         //waitKey(1);
        // t2 = chrono::high_resolution_clock::now();
//        cout << "Capture period: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;
//        cout << endl;
    }
}

}
